{{PAHS Robotics Operating System Functions}}

CON
  ServoCh1 = 16  
  ServoCh2 = 17
  ServoCh3 = 18
  ServoCh4 = 19

  Forward  = 1
  Reverse  = 2
  Right    = 3
  Left     = 4
  Stop     = 5
  
VAR
  byte state

OBJ
  pst    : "Parallax Serial Terminal"
  servo  : "Servo32v7.spin"
  
PUB initialize
  servo.Start
  
PUB LED(pin, OnOff)
  dira[pin] := outa[pin] := OnOff
  repeat

PUB RecvState(pin)
  state := ina[pin]                        'When run, the output should look like the following in the 
  pst.str(string("State of pin "))         'Parallax Serial Terminalwhen pin = 16 and the state = 1:
  pst.str(pin)                             'State of pin 16: 1
  pst.str(string(" : "))
  pst.str(state)
  pst.str(string(13))

PUB Drive(Direction)
 case Direction
    Forward: servo.Set(ServoCh1,2000)
      servo.Set(ServoCh2,2000)
      servo.Set(ServoCh3,1000)
      servo.Set(ServoCh4,1000) 
    Reverse: servo.Set(ServoCh1,1000)
      servo.Set(ServoCh2,1000)
      servo.Set(ServoCh3,2000)
      servo.Set(ServoCh4,2000)
    Right: servo.Set(ServoCh1,2000)
      servo.Set(ServoCh2,2000)
      servo.Set(ServoCh3,2000)
      servo.Set(ServoCh4,2000) 
    Left: servo.Set(ServoCh1,1000)
      servo.Set(ServoCh2,1000)
      servo.Set(ServoCh3,1000)
      servo.Set(ServoCh4,1000)
    Stop: servo.Set(ServoCh1,1500)
      servo.Set(ServoCh2,1500)
      servo.Set(ServoCh3,1500)
      servo.Set(ServoCh4,1500)

      